﻿

namespace PLCSharp.CoreModel.Tools
{
    public class XPoint
    {

        public double X { get; set; }
        public double Y { get; set; }
        public double Z { get; set; }
        public double Angle { get; set; }

        public static bool Equals(XPoint point1, XPoint point2)
        {
            return point1.X == point2.X 
                && point1.Y == point2.Y 
                && point1.Angle == point2.Angle;
        }
 

        public static XPoint GetOffsPoint(XPoint orign, double angle, XPoint before)
        {
            XPoint to = new();
            var t = Math.Atan2((orign.Y - before.Y), (orign.X - before.X)) * 180 / Math.PI;
            to.Angle = t + angle;
            var angleH = angle * Math.PI / 180;
            var x = before.X - orign.X;
            var y = before.Y - orign.Y;

            to.X = x * Math.Cos(angleH) + y * Math.Sin(angleH) + orign.X;
            to.Y = -x * Math.Sin(angleH) + y * Math.Cos(angleH) + orign.Y;
            return to;
        }
        public override string ToString()
        {
            return $"X:{X:F4},Y:{Y:F4},Angle:{Angle:F4}";
        }

    }
}
